基于力传感器的关节导纳拖动功能基础指令集介绍
| 修订日期 | 修订版本 | 修订内容 | 修订人 |
|---|---|---|---|
| 2024.7.23 | V1.0 | 初始化文档 | 钟一辰 |
[TOC]
1 接口介绍
具体例程可参考demo
1.1 参数设置及工具标定（开启力控前）
1）创建算法库实例
/**
* @brief 创建机器人算法库实例
* @param modelName：机器人对应的 URDF 描述文件
* @param path: 机器人资源文件的路径
* @return 算法库实例指针,如果失败, 返回 nullptr
* ARAL算法库文件可以通过 dlopen 在运行时打开, 通过调用 CreateRLIntfacePtr 函数得到基类接口指针
*/
ARAL::interface::ARALIntfacePtr CreateARALIntfacePtrFromFile(const char* model_name, const char* path);
2）设置传感器位姿
/**
* @brief 设置末端传感器坐标系在机器人法兰坐标系下的位姿
* @param pose: 位置和姿态数组信息
* @return if < 0, 表示设置失败
*/
ARAL_API_COMMON(1.0) int mdlSetEndSensorPoseInFlange(const RLPose& pose) = 0;
3）设置工具末端位姿
/**
* @brief 设置工具坐标系的位姿(调用此函数会触发规划初始点状态的更新)
* 1. 如果是末端工具,则为 工具 相对于法兰坐标系的位姿
* 2. 如果是远端工具,则为 工具 相对于基坐标系的位姿, 需要先调用 mdlEnableRemoteTool 来使能远端工具
* @param pose: 位置和姿态数组信息
* @return if < 0, 表示设置失败
*/
ARAL_API_COMMON(1.0) int mdlSetToolPose(const RLPose& pose) = 0;
负载信息可选择使用CAD模型或三点标定法
4.1）若使用CAD模型，需提供负载质量、质心及传感器偏置等信息；
4.2）若使用三点标定法，调用以下接口
/**
* @brief 根据末端 6 维力/力矩传感器的信息标定工具和传感器的属性( 3 点法), 只能标定工具的质量和质心在法兰坐标系的描述
* @param T_b_f: 输入多种不同构型对应的法兰在基坐标系的描述, 这几种构型的姿态差异越大越好（个数应不小于 3）
* @param mesureData: 在对应构型下测得的传感器的原始数据
* @param res: 标定结果(见参考文档)
* @return 返回值 < 0, 表示计算失败
*/
ARAL_API_COMMON(1.0) int calibToolDynamicParameterWithFTSensor(const std::vector<RLPose>& T_b_f, const std::vector<RLWrench>& mesureData, FtSensorCalibResult& res) = 0;
5） 负载惯量信息均设为0，调用以下接口设置负载动力学参数
/**
* @brief 设置负载的动力学参数, 参考坐标系为末端法兰坐标系
* @param inertial: 负载的动力学参数结构体
* @return < 0, 表示设置失败
*/
ARAL_API_COMMON(1.0) int mdlSetLoadDynamicParameterInFlange(const RLInertia& inertia) = 0;
6）设置控制功能类型，因为是关节导纳，所以这里的ControlMode选择JNT_ADM_FREE_DRIVE
/**
* @brief 设置控制类型
* @param type:主要有三种类型: 1）位置控制; 2）导纳控制; 3）阻抗控制
* 主类型下面又分一些子类型, 如导纳控制目前支持 0:拖动示教; 1:轨迹跟踪(柔顺); 2:恒力跟踪等功能
* 子类型无需用户显式指定, 而是根据用户设置的不同参数调整算法控制器结构来实现对应的功能
* @return 返回设置状态
*/
ARAL_API_COMMON(1.0) int mcSetControlType(const ControlMode& type) = 0;
7）初始化算法库状态，因为是关节导纳，所以在调用mcInitiateControlPara接口时，DescribeSpace应该设置为JOINT
/**
* @brief 初始化规划器状态(根据机械臂的实际状态更新规划起始点), 遇到以下情形需要调用本函数:
* 1) 第一次创建规划器;
* 2) 规划返回错误, 错误处理(一般调用急停函数)完成后重新初始化;
* 3) 切换控制器(机械臂的运动由另外规划器规划)后重新初始化(可以初始化成 IDEL 和 PAUSED 两种状态)
* @param q: 机械臂规划起始关节角
* @param qd: 机械臂规划起始关节速度
* @param qdd: 机械臂规划起始关节加速度
* @param ToolWorkpiece: 当前运动对应的工具和工件信息
* @param status: 将规划器初始化成何种状态(目前只支持 IDEL 和 PAUSED(只支持将规划器当前状态初始化成暂停状态) 两种)
* @return if < 0, 则表示设置失败
*/
ARAL_API_COMMON(1.0) int rsInitiateRobotState(const RLJntArray& q, const RLJntArray& qd, const RLJntArray& qdd, const ToolWorkpiece& tool_workpiece, const PlannerStatus& status) = 0;
/**
* @brief 初始化控制器参数
* @param space: 控制坐标系类型
* @param frame: 如果控制在笛卡尔空间进行, 本参数为任务坐标系相对于基坐标系的位姿
* @return: 返回值 < 0 表示设置失败
*/
ARAL_API_COMMON(1.0) int mcInitiateControlPara(const DescribeSpace& space, const RLPose& frame) = 0;
注意！
a）必须先设置控制功能类型，再进行初始化
b）调用rsInitiateRobotState时，q,qd,qdd务必为开启力控前第一个点的关节角度，速度，加速度
8）设置阻抗质量，阻尼，刚度参数，因为是关节导纳，这里的DescribeSpace应该设置为JOINT
/**
* @brief 设置阻抗控制质量参数, 参数的数值必须是正数
* @param space: 空间选择, 取值 JOINT 或 CARTESIAN, 其他非法
* @param data: 质量，笛卡尔空间单位kg，长度为工作空间维度（一般是6），顺序为[x,y,z,Rx,Ry,Rz]
* 关节空间单位kg*m^2，长度为机器人自由度（一般是6），顺序为[J1,J2,J3,J4,J5,J6]
* 质量数值越大，相同加速度产生的阻力/力矩越大。
* @return: 返回设置状态
*/
ARAL_API_COMMON(1.0) int fcSetMass(const DescribeSpace& space, const double* mass) = 0;
/**
* @brief 设置阻抗控制阻尼参数, 参数的数值必须是正数
* @param space: 空间选择, 取值 JOINT 或 CARTESIAN, 其他非法
* @param damp: 阻尼，笛卡尔空间单位N*s/m，长度为工作空间维度（一般是6），顺序为[x,y,z,Rx,Ry,Rz]
* 关节空间单位Nm*s/rad，长度为机器人自由度（一般是6），顺序为[J1,J2,J3,J4,J5,J6]
* 阻尼数值越大，相同速度产生的阻力/力矩越大。
* @return: 返回设置状态
*/
ARAL_API_COMMON(1.0) int fcSetDamp(const DescribeSpace& space, const double* damp) = 0;
/**
* @brief 设置阻抗控制刚度参数, 参数的数值必须是正数
* @param space: 空间选择, 取值 JOINT 或 CARTESIAN, 其他非法
* @param stiffness: 刚度，笛卡尔空间单位N/m，长度为工作空间维度（一般是6），顺序为[x,y,z,Rx,Ry,Rz]
* 关节空间单位Nm/rad，长度为机器人自由度（一般是6），顺序为[J1,J2,J3,J4,J5,J6]
* 刚度数值越大，力控参考坐标系距离参考位置越远，产生的力/力矩越大。
* @return: 返回设置状态
*/
ARAL_API_COMMON(1.0) int fcSetStiffness(const DescribeSpace& space, const double* stiffness) = 0;
注意！
刚度参数必须设成0，以下参数设置仅供参考：
Mass = [5, 5, 5, 5, 5, 5]
Damp = [100, 100, 100, 100, 100, 100]
Stiffness = [0, 0, 0, 0, 0, 0]
9）设置力外环PID增益，其中P必须设为1，其他都设为0
/**
* @brief 设置力外环控制律的调节增益值
* @param P: 力外环比例增益，长度为工作空间维度（一般是6），顺序为[x,y,z,Rx,Ry,Rz]
* @param I: 力外环积分增益，长度为工作空间维度（一般是6），顺序为[x,y,z,Rx,Ry,Rz]
* @param D: 力外环微分增益，长度为工作空间维度（一般是6），顺序为[x,y,z,Rx,Ry,Rz]
*/
ARAL_API_COMMON(1.0) int fcSetForceLoopPIDGains(const double* P, const double* I, const double* D) = 0;
10）设置力控方向，这里的DescribeSpace应该设置为JOINT
/**
* @brief 设置力控选择方向向量
* @param space: 空间选择, 取值 JOINT 或 CARTESIAN, 其他非法
* @param value: 方向向量, 元素取值0为关闭对应维度力控, 1为开启对应维度力控;
* 长度为工作空间维度或机器人自由度，顺序为[x,y,z,Rx,Ry,Rz]或[J1,J2,J3,J4,J5,J6]
* @return 返回值 < 0 表示设置失败
*/
ARAL_API_COMMON(1.0) int fcSetSelectionVector(const DescribeSpace& space, const int* value) = 0;
11）设置力控响应阈值，这里的DescribeSpace应该设置为JOINT
/**
* @brief 设置控制算法对输入的响应阈值
* @param space: 空间选择, 取值 JOINT 或 CARTESIAN, 其他非法
* @param threshold:阈值, 笛卡尔空间单位N、Nm，长度为工作空间维度, 顺序为[x,y,z,Rx,Ry,Rz]
* 关节空间单位Nm，长度为机器人自由度，顺序为[J1,J2,J3,J4,J5,J6]
* 当作用于控制算法的数值超过该值时，算法才有效。(该值为非负值)
* @return 返回值 < 0 表示设置失败
*/
ARAL_API_COMMON(1.0) int fcSetForceThreshold(const DescribeSpace& space, const double* threshold) = 0;
12）设置力控响应限制，这里的DescribeSpace应该设置为JOINT
/**
* @brief 设置控制算法对输入的响应限制值
* @param space: 空间选择, 取值 JOINT 或 CARTESIAN, 其他非法
* @param data: 限制值，如果超过该值时，会截断数据，按照最大值计算。(该值为正值)
* 笛卡尔空间单位N、Nm，长度为工作空间维度（一般是6），顺序为[x,y,z,Rx,Ry,Rz]
* 关节空间单位Nm，长度为机器人自由度（一般是6），顺序为[J1,J2,J3,J4,J5,J6]
* @return 返回值 < 0 表示设置失败
*/
ARAL_API_COMMON(1.0) int fcSetForceLimit(const DescribeSpace& space, const double* data) = 0;
1.3 运动至一个不奇异的位姿
注意！
a）开启力控前机器人必须运动至一个适合拖动的安全点（这个用户可根据经验调整），以保证开始拖动的初始位姿不在奇异防护的范围之内。
b）该点需与rsInitiateRobotState接口初始化的点保持一致
1.4 开启力控
本步骤为实时接口，循环调用，尽量避免耗时操作
1）更新机器人运动状态
/**
* @brief 更新机器人关节的实时状态信息。(如果用户不能给出全部信息, 可以先调用 utlKalmanFilterPVA 函数进行估计)
* 调用这个接口会连带更新机械臂所有连杆在笛卡尔空间的运动学信息, 包括位姿, 速度和加速度等
* @param q: 关节当前位置
* @param qd: 关节当前速度
* @param qdd: 关节当前加速度
* @param is_user_acc: 是否采用用户输入的加速度(默认是否)
* @return if < 0, 表示设置失败
*/
ARAL_API_COMMON(1.0) int rsUpdateJointPVA(const RLJntArray& q, const RLJntArray& qd, const RLJntArray& qdd, const bool& is_user_acc = false) = 0;
2）更新传感器数据
/**
* @brief 更新力传感器的信息, 处于力控模式时每个周期都需更新(传感器的偏置需要在软件层处理)
* (！！！在使用时先更新机械臂的状态, 再更新传感器的状态)
* @param type: END_FT_SENSOR 或者 BASE_FT_SENSOR 或者 JOINT_FT_SENSOR
* @param ftData: 传感器在每个维度方向的测量数据
* @return if < 0, 表示设置失败
*/
ARAL_API_COMMON(1.0) int rsUpdateFTSensorData(const FTSensorType& type, const double* ftData) = 0;
注意！
该接口的 ftData 数据应为传感器实际数据减去偏置数据，若无法从外设模块设入，则需在设入该接口前自行减去
3）更新动力学数据
/**
* @brief 更新机器人关节的实时关节力矩/摩擦和温度信息, 用于碰撞监测或估计外力
* @param toruqe: 关节实际力矩, 单位Nm
* @param temperature: 关节温度, 单位为摄氏度(℃)
* @param fricition: 关节摩擦力，单位为Nm
* @return if < 0, 表示设置失败
*/
ARAL_API_COMMON(1.0) int rsUpdateJointCTF(const RLJntArray& toruqe, const RLJntArray& temperature, const RLJntArray& friction) = 0;
4）设置参考轨迹，因为选择的是关节导纳拖动功能，所以参考轨迹的位置，速度，加速度信息应设为空
/**
* @brief 设置参考轨迹1(用户设置参考位置时，必须同时设置参考速度和参考加速度;否则可以都设置为空)
* @param type: 参考轨迹描述坐标系,具体可参考 DescribeSpace 定义
* @param frame: 如果type不等于JOINT, 则frame表示参考轨迹坐标系相对基坐标系的相对位姿关系
* @param positions: 参考轨迹的位置信息, 如没有, 则置空
* @param velocities: 参考轨迹的速度信息, 如没有, 则置空
* @param accelerations: 参考轨迹的加速度信息, 如没有, 则置空
* @return if < 0, 则表示设置失败
*/
ARAL_API_COMMON(1.0) int rsSetReferenceTrajectory(const DescribeSpace& type, const RLPose& frame, const double* positions,
const double* velocities, const double* accelerations) = 0;
5）计算控制输出
/**
* @brief 计算关节的控制指令参数.(调用该函数时, 如果算法出错, 则返回数据不可用)
* @param res: 返回值, 包括关节位置、速度、加速度和力矩
* @param ik_eps: 逆解迭代精度参数
* @return return: 0 - 计算正确
* -60003 - 运动速度超限，解决方法：调整力控参数
* 65001 - 机器人没有参考轨迹，解决方法：调用rsSetReferenceTrajectory设置参考轨迹
* -30027 - 没有和参考角在同一关节子空间的逆解，解决方法：未实现在子空间内搜索逆解, 建议更改选解条件，RobotConfiguration::NONE
* -30006 - IK 计算失败；解决方法：机械臂在当前位置没有解，且没有使能迭代解, 建议使能迭代解
* -40055 - 速度规划失败，解决方法：检查始末速度是否超过运动约束、检查运动约束是否大于0
*/
ARAL_API_COMMON(1.0) int rsCalJointCommand(JointCommand& result, const Array2d& ik_eps = {ARAL_IK_ITER_POS_EPS, ARAL_IK_ITER_ORI_EPS}) = 0;